/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          :
 * Description        :
 ******************************************************************************
 * @attention
 *
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "main.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
PIDControlStruct PIDControl_IQ;
PIDControlStruct PIDControl_ID;
PIDControlStruct PIDControl_V;
PIDControlStruct PIDControl_P;
/* USER CODE END Variables */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void PID_Default(void)
{
	PIDControl_IQ.kp = 0.5f;
	PIDControl_IQ.ki = 0.1f;
	PIDControl_IQ.kd = 0.0f;
	PIDControl_IQ.upper = 20.0f * Amp_Num_SCALE;
	PIDControl_IQ.lower = -20.0f * Amp_Num_SCALE;

	PIDControl_ID.kp = 0.5f;
	PIDControl_ID.ki = 0.1f;
	PIDControl_ID.kd = 0.0f;
	PIDControl_ID.upper = 20.0f * Amp_Num_SCALE;
	PIDControl_ID.lower = -20.0f * Amp_Num_SCALE; // ampere

	PIDControl_V.kp = 1.86f;
	PIDControl_V.ki = 0.0019f;
	PIDControl_V.kd = 0.0093f;
	PIDControl_V.upper = 5.0f * SPEED_PID_GAIN; // ampere
	PIDControl_V.lower = -5.0f * SPEED_PID_GAIN;
}

void PID_Parameters_Update(void)
{
	PIDControl_IQ.kp = g_sys.RW.UserPara.Para.Pid_Q.kp;
	PIDControl_IQ.ki = g_sys.RW.UserPara.Para.Pid_Q.ki;
	PIDControl_IQ.kd = g_sys.RW.UserPara.Para.Pid_Q.kd;
	PIDControl_IQ.upper = g_sys.RW.UserPara.Para.Pid_Q.upper * Amp_Num_SCALE;
	PIDControl_IQ.lower = g_sys.RW.UserPara.Para.Pid_Q.lower * Amp_Num_SCALE;

	PIDControl_ID.kp = g_sys.RW.UserPara.Para.Pid_D.kp;
	PIDControl_ID.ki = g_sys.RW.UserPara.Para.Pid_D.ki;
	PIDControl_ID.kd = g_sys.RW.UserPara.Para.Pid_D.kd;
	PIDControl_ID.upper = g_sys.RW.UserPara.Para.Pid_D.upper * Amp_Num_SCALE;
	PIDControl_ID.lower = g_sys.RW.UserPara.Para.Pid_D.lower * Amp_Num_SCALE; // ampere

	PIDControl_V.kp = g_sys.RW.UserPara.Para.Pid_V.kp;
	PIDControl_V.ki = g_sys.RW.UserPara.Para.Pid_V.ki;
	PIDControl_V.kd = g_sys.RW.UserPara.Para.Pid_V.kd;
	PIDControl_V.upper = g_sys.RW.UserPara.Para.Pid_V.upper * SPEED_PID_GAIN; // ampere
	PIDControl_V.lower = g_sys.RW.UserPara.Para.Pid_V.lower * SPEED_PID_GAIN;

	PIDControl_P.kp = g_sys.RW.UserPara.Para.Pid_P.kp;
	PIDControl_P.ki = g_sys.RW.UserPara.Para.Pid_P.ki;
	PIDControl_P.kd = g_sys.RW.UserPara.Para.Pid_P.kd;
	PIDControl_P.upper = g_sys.RW.UserPara.Para.Pid_P.upper; // rpm/min
	PIDControl_P.lower = g_sys.RW.UserPara.Para.Pid_P.lower;
}

void PID_Controller_Reset(void)
{
	PID_Parameters_Update();

	PIDControl_IQ.last = 0.0f;
	PIDControl_IQ.previous = 0.0f;
	PIDControl_IQ.integral = 0.0f;
	PIDControl_IQ.out = 0.0f;

	PIDControl_ID.last = 0.0f;
	PIDControl_ID.previous = 0.0f;
	PIDControl_ID.integral = 0.0f;
	PIDControl_ID.out = 0.0f;

	PIDControl_V.last = 0.0f;
	PIDControl_V.previous = 0.0f;
	PIDControl_V.integral = 0.0f;
	PIDControl_V.out = 0.0f;

	PIDControl_P.last = 0.0f;
	PIDControl_P.previous = 0.0f;
	PIDControl_P.integral = 0.0f;
	PIDControl_P.out = 0.0f;
}

float PID_Controller(PIDControlStruct *pid, float error)
{
	float result;

	float dError = error - pid->last;

	float LimitUpeer = pid->upper / pid->ki;
	float LimitLower = pid->lower / pid->ki;
	pid->integral += error;
	utils_limit_float(&pid->integral, LimitLower, LimitUpeer);

	result = (pid->kp * error) + (pid->ki * pid->integral) + (pid->kd * dError);
	if (result > pid->upper)
	{
		result = pid->upper;
		// pid->integral +=	(result - pid->upper);
	}
	else if (result < pid->lower)
	{
		result = pid->lower;
		// pid->integral +=	(result - pid->lower);
	}

	// save last error
	pid->previous = pid->last;
	pid->last = error;

	return result;
}

float IncPID_Controller(PIDControlStruct *pid, float error)
{
	static char first_calc = 0;
	if (!first_calc)
	{
		first_calc = 1;
		return 0;
	}
	float result = (pid->kp * (error - pid->last)) + (pid->ki * error) + (pid->kd * (error + pid->previous - (2 * pid->last)));
	pid->out += result;
	if (pid->out > pid->upper)
	{
		pid->out = pid->upper;
		// pid->integral +=	(result - pid->upper);
	}
	else if (pid->out < pid->lower)
	{
		pid->out = pid->lower;
		// pid->integral +=	(result - pid->lower);
	}

	// save last error
	pid->previous = pid->last;
	pid->last = error;

	return pid->out;
}

/* USER CODE END Application */
